use core::{
    ffi::{c_char, c_void},
    marker::PhantomData,
    ptr::{addr_of, addr_of_mut, null_mut},
};

use crate::{
    c::{
        osMessageQueueAttr_t, osMessageQueueGet, osMessageQueueId_t, osMessageQueueNew,
        osMessageQueuePut, osRtxMessageQueue_t, osStatus_t_osOK,
    },
    osWaitForever,
    utils::Store,
};

const CB_SIZE: usize = size_of::<osRtxMessageQueue_t>();

pub struct MessageQueue<T, const MSG_COUNT: usize, const MQ_SIZE: usize> {
    _p: PhantomData<T>,
    cb_mem: Store<CB_SIZE>,
    mq_mem: Store<MQ_SIZE>,
    id: osMessageQueueId_t,
}

/**
#define osRtxMessageQueueMemSize(msg_count, msg_size) \
  (4*(msg_count)*(3+(((msg_size)+3)/4)))
 */

pub const fn msg_queue_mem_size(count: usize, size: usize) -> usize {
    (4 * count) * (3 + ((size + 3) / 4))
}

impl<T: Copy + Default, const MSG_COUNT: usize, const MQ_SIZE: usize>
    MessageQueue<T, MSG_COUNT, MQ_SIZE>
{
    const MSG_SIZE: usize = size_of::<T>();

    pub const fn const_new() -> Self {
        Self {
            _p: PhantomData,
            cb_mem: Store::const_new(),
            mq_mem: Store::const_new(),
            id: null_mut(),
        }
    }

    pub fn create(&mut self, name: &str) {
        let attr = osMessageQueueAttr_t {
            name: name.as_ptr() as *const c_char,
            attr_bits: 0,
            cb_mem: self.cb_mem.c_ptr(),
            cb_size: CB_SIZE as u32,
            mq_mem: self.mq_mem.c_ptr(),
            mq_size: MQ_SIZE as u32,
        };
        unsafe {
            self.id = osMessageQueueNew(MSG_COUNT as u32, Self::MSG_SIZE as u32, &attr);
        }
        debug_assert!(self.id != null_mut(), "msg queue create fail");
    }

    pub fn put(&self, msg: T) {
        unsafe {
            osMessageQueuePut(self.id, addr_of!(msg) as *const c_void, 0, 0);
        }
    }

    pub fn poll(&self) -> T {
        let mut value: T = T::default();
        unsafe {
            osMessageQueueGet(
                self.id,
                addr_of_mut!(value) as *mut c_void,
                null_mut(),
                osWaitForever,
            );
        }
        value
    }

    pub fn poll_with_timeout(&self, timeout: u32) -> Option<T> {
        let mut value: T = T::default();
        let ret = unsafe {
            osMessageQueueGet(
                self.id,
                addr_of_mut!(value) as *mut c_void,
                null_mut(),
                timeout,
            )
        };
        if ret != osStatus_t_osOK {
            None
        } else {
            Some(value)
        }
    }
}
